#pragma once

#include <stdio.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "../Geometry/Vector3.h"
#include "../Geometry/BoundingBox.h"
#include "../Geometry/Point.h"
#include "../System/Platform.h"
#include "Shape.h"

#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem.hpp>

namespace PCS
{
	namespace Core
	{			
		class PointCloud
			: public IShape
		{
		protected:
			AxisAlignedBoundingBox* m_pBoundingBox;
			std::vector<Point*> m_points;			
			std::string m_filepath; //filepath of original (full point cloud) binary file
			size_t m_offset;      //segment offset within file in bytes.
			size_t m_offset_idx;  //offset within the current loaded segment in sizeof(float)
			size_t m_global_idx;  //global index for each point  ... 
			size_t m_size;        //number of points in the cloud. 
			size_t m_segments;    //number of segments required to load file within memory bounds.
			size_t m_active_segment;    //currently loaded active segment
			size_t m_segment_size;	    //segment size in sizeof(float)
			size_t m_trailing_segment_size;	 //	final segment size in sizeof(float)
			boost::interprocess::mapped_region *m_region;
			boost::interprocess::file_mapping m_file;
			float * r_addr; // start of m_region
			
		public:
			PointCloud(void) : IShape() { }			
			PointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr p_pcdCloud, const std::string& p_strName) : IShape(p_strName) {
				size_t p_size = p_pcdCloud->size();
				m_points.resize(p_size);
				//iterate through the pcd file and populate the m_points vector.				
				for (size_t i = 0; i<p_size; i++) {
					m_points[i] = new Point(p_pcdCloud->points[i].x, p_pcdCloud->points[i].y, p_pcdCloud->points[i].z);
				}
				ComputeBoundingVolume();
				TranslateToOrigin();
			}

			PointCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> * p_pcdClouds, const std::string& p_strName) : IShape(p_strName) {				
				size_t p_size = 0;
				//Determine total number of points in vector of pointclouds.
				for (std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>::iterator cloudsIterator = p_pcdClouds->begin(); cloudsIterator != p_pcdClouds->end(); ++cloudsIterator) {
					p_size += (*cloudsIterator)->size();
				}
				//Set the vector size once.
				m_points.resize(p_size);
				//Populate the vector
				size_t idx = 0;
				for (std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>::iterator cloudsIterator = p_pcdClouds->begin(); cloudsIterator != p_pcdClouds->end(); ++cloudsIterator) {
					size_t p_size_crt = (*cloudsIterator)->size();
					//iterate through the pcd file and populate the m_points vector.				
					for (int i = 0; i<p_size_crt; i++, idx++) {
						m_points[idx] = new Point((*cloudsIterator)->points[i].x, (*cloudsIterator)->points[i].y, (*cloudsIterator)->points[i].z);						
					}					
				}
				ComputeBoundingVolume();
				TranslateToOrigin();
			}

			PointCloud(const std::string& p_strName) : IShape(p_strName) {				
				size_t p_size = 0;
				m_filepath = p_strName;
				//Determine the size of the file ... i.e. number of points * 3 * sizeof(float)
				boost::filesystem::path p (p_strName, boost::filesystem::native);
				if ( !boost::filesystem::exists( p ) )
				{
					std::cout << "not found: " << p_strName << std::endl;
				}

				if ( !boost::filesystem::is_regular( p ) )
				{
					std::cout << "not a regular file: " << p_strName << std::endl;
				}

				size_t f_size = boost::filesystem::file_size( p );
				p_size = f_size / (3 *sizeof(float));
				m_size = p_size;

				std::cout << "[PointCloud] Size of file is " << boost::filesystem::file_size( p ) << ". Number of points = " <<  p_size << std::endl;			

				//Set the vector size.
				if (p_size < MEM_UPPERBOUND)
				{
					//all points fit in main memory
					const char * filename = p_strName.c_str();
					m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);
					
					r_addr = (float*)m_region->get_address();
					size_t r_size = m_region->get_size();					
					m_segments = 1;
					m_segment_size = p_size * 3;
					m_active_segment = 0;
					std::cout << "[PointCloud] Loaded All Points. Size=" << r_size << " #segments=" << m_segments << " #points=" << m_segment_size << std::endl;
					//Compute Bounding Volume
					m_pBoundingBox = new AxisAlignedBoundingBox();
					m_pBoundingBox->ComputeFromPoints(r_addr, p_size);
				}
				else 
				{					
					const char * filename = p_strName.c_str();
					m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, 0, MEM_UPPERBOUND * 3 * sizeof(float));

					r_addr = (float*)m_region->get_address();
					size_t r_size = m_region->get_size();
					m_segment_size = MEM_UPPERBOUND * 3;
					m_active_segment = 0;  //segments start from 0.
					m_segments = (p_size / MEM_UPPERBOUND);
					m_trailing_segment_size = p_size % MEM_UPPERBOUND;
					if (m_trailing_segment_size > 0) m_segments++;
					m_trailing_segment_size = m_trailing_segment_size * 3;
					std::cout << "[PointCloud] Loaded First Segment. Size=" << r_size << " #segments=" << m_segments << " #points=" << m_segment_size << std::endl;					
					//Compute Bounding Volume
					m_pBoundingBox = new AxisAlignedBoundingBox();
					m_pBoundingBox->ComputeFromPoints(r_addr, m_segment_size/3);
					size_t offset = 0;
					size_t s_size = 0;
					for (int i = 1; i<m_segments-1; i++) {
						m_active_segment++;
						delete m_region;
						offset = ((m_active_segment) * (MEM_UPPERBOUND * 3 * sizeof(float)));
						s_size = MEM_UPPERBOUND * 3 * sizeof(float);
						m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, offset, s_size);
						r_addr = (float*)m_region->get_address();
						AxisAlignedBoundingBox aabBox = AxisAlignedBoundingBox();
						aabBox.ComputeFromPoints(r_addr, m_segment_size/3);
						m_pBoundingBox->Union(aabBox);
					}
					//load the trailing final segment, compute local BV and union it with the global BV
					m_active_segment++;
					delete m_region;
					offset = ((m_active_segment) * (MEM_UPPERBOUND * 3 * sizeof(float)));
					s_size = (m_trailing_segment_size * sizeof(float)) -1;
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, offset, s_size);
					r_addr = (float*)m_region->get_address();
					AxisAlignedBoundingBox aabBox = AxisAlignedBoundingBox();
					aabBox.ComputeFromPoints(r_addr, m_trailing_segment_size/3);
					m_pBoundingBox->Union(aabBox);
					delete m_region;
					//A region is always loaded in memory ... Load first segment here.					
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, 0, MEM_UPPERBOUND * 3 * sizeof(float));
					r_addr = (float*)m_region->get_address();
					m_active_segment = 0;
					m_offset = 0;
					m_offset_idx = 0;
					m_global_idx = 0;
					m_segment_size = MEM_UPPERBOUND * 3; 
				}
												
			}

			~PointCloud(void) {
				for (std::vector<Point*>::iterator pointIterator = m_points.begin(); 
					 pointIterator != m_points.end(); ++pointIterator)
				{
					delete *pointIterator;
				}

				m_points.clear();
			}	

			std::string GetFilepath(void) {
				return m_filepath;
			}

			bool GetNext(Vector3 &p_next) {
				if (m_offset_idx < m_segment_size)
				{
					//simply update p_next with the next 3 floats
					p_next.X = r_addr[m_offset_idx]; m_offset_idx++;
					p_next.Y = r_addr[m_offset_idx]; m_offset_idx++;
					p_next.Z = r_addr[m_offset_idx]; m_offset_idx++;
					m_global_idx++;
					return true;
				}
				else 
				{
					if (m_active_segment+1 < m_segments) 
					{
						//done with this segment but there are others to look at
						delete m_region;
						m_active_segment++;
						size_t offset;
						size_t s_size;
						if (m_active_segment == m_segments-1)
						{
							//loading final trailing segment
							offset = (m_active_segment) * (MEM_UPPERBOUND * 3 * sizeof(float));
							s_size = (m_trailing_segment_size * sizeof(float)) -1;
							m_segment_size = m_trailing_segment_size;   //number of floats in this final segment
						} 
						else
						{
							//not at the end ... load a full segment
							offset = (m_active_segment) * (MEM_UPPERBOUND * 3 * sizeof(float));
							s_size = MEM_UPPERBOUND * 3 * sizeof(float);
							m_segment_size = MEM_UPPERBOUND * 3;   //number of floats in this final segment
						}						
						m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, offset, s_size);
						r_addr = (float*)m_region->get_address();
						m_offset_idx = 0;												
						p_next.X = r_addr[m_offset_idx]; m_offset_idx++;
						p_next.Y = r_addr[m_offset_idx]; m_offset_idx++;
						p_next.Z = r_addr[m_offset_idx]; m_offset_idx++;
						m_global_idx++;
						return true;
					}
					else 
					{
						//we're done .... return false;						
						return false;
					}
				}
				return false;
			}

			void Reset(void) {
				//Load first region and reset all indices.
				if (m_segments == 1) 
				{
					//just reset the offset idx ... 
					m_offset_idx = 0;
					m_global_idx = 0;
				}
				else 
				{
					delete m_region;
					m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only, 0, MEM_UPPERBOUND * 3 * sizeof(float));
					r_addr = (float*)m_region->get_address();
					m_segment_size = MEM_UPPERBOUND * 3;
					m_active_segment = 0;  //segments start from 0.
					m_offset_idx = 0;
					m_global_idx = 0;
				}
			}

			bool IsBounded(void) const { return true; }

			void ComputeBoundingVolume(void) {				
				m_pBoundingBox = new AxisAlignedBoundingBox(Vector3(Maths::Maximum), Vector3(Maths::Minimum));
				m_pBoundingBox->ComputeFromPoints(m_points);
			}

			IBoundingVolume* GetBoundingVolume(void) const { return m_pBoundingBox; }

			size_t GetSize(void) { if (m_size==0) return m_points.size(); else return m_size; }

			std::vector<Point*>* GetPoints(void) { return &m_points; }

			Point* GetPointAt(size_t _index) { return m_points[_index]; }

			/**Calculate the median along the X,Y,Z coordinates and translate it over 0, 0, 0*/
			void TranslateToOrigin() {
				for (std::vector<Point*>::iterator pointIterator = m_points.begin(); 
					 pointIterator != m_points.end(); ++pointIterator)
				{
					(*pointIterator)->position.X -= m_pBoundingBox->GetCentre().X;
					(*pointIterator)->position.Y -= m_pBoundingBox->GetCentre().Y;
					(*pointIterator)->position.Z -= m_pBoundingBox->GetCentre().Z;
				}

				Vector3 newMinExtent = Vector3(m_pBoundingBox->GetMinExtent().X - m_pBoundingBox->GetCentre().X,
													m_pBoundingBox->GetMinExtent().Y - m_pBoundingBox->GetCentre().Y,
													m_pBoundingBox->GetMinExtent().Z - m_pBoundingBox->GetCentre().Z);

				Vector3 newMaxExtent = Vector3(m_pBoundingBox->GetMaxExtent().X - m_pBoundingBox->GetCentre().X,
													m_pBoundingBox->GetMaxExtent().Y - m_pBoundingBox->GetCentre().Y,
													m_pBoundingBox->GetMaxExtent().Z - m_pBoundingBox->GetCentre().Z);

				m_pBoundingBox->SetMinExtent(newMinExtent);
				m_pBoundingBox->SetMaxExtent(newMaxExtent);			
			}

			void WriteFromPCDASCIIToPCSBinFormat(std::string _filepath) {				
				const char * filename = _filepath.c_str();
				size_t pc_size = m_points.size();
				std::filebuf fbuf;
				fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
				fbuf.pubseekoff((pc_size*3*sizeof(float))-sizeof(char), std::ios_base::beg);				
				fbuf.sputc(0);
				fbuf.close();

				boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
				boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

				float * r_addr = (float*)region.get_address();			
				size_t r_size = region.get_size();				
				std::cout << "[PointCloud] Writing File - MMF created and mapped to region. Size= " << r_size << std::endl;
				size_t idx = 0;
				
				for (int i = 0; i<pc_size-1; i++) {					
					r_addr[idx] = m_points[i]->position.X; idx++;
					r_addr[idx] = m_points[i]->position.Y; idx++;
					r_addr[idx] = m_points[i]->position.Z; idx++;
				}

				region.flush();
			}

			std::string ToString(void) { return boost::str(boost::format("PCloud # : %d, BV : %s") % m_points.size() % m_pBoundingBox->ToString()); }

		};			
	}
}
